/*
 * Axis.h
 *
 *  Created on: 21.01.2021
 *      Author: Yannick / Lidders
 */

#ifndef SRC_AXIS_H_
#define SRC_AXIS_H_
#include "usb_hid_ffb_desc.h"
#include "HidFFB.h"
#include "ffb_defs.h"
#include "EffectsCalculator.h"
#include "FastAvg.h"

#define ET_MAX_POWER		0x7FFF		// 游戏给出的特效力矩，有正负两个方向
#define ET_MAX_POSTION		0x7FFF		// 游戏里最大位置,有正负两个方向
#define MOTOR_MAX_POWER		2000		// 电机的最大力矩，1‰，有正负两个方向
#define MIN_DEGREES			90
#define MAX_DEGREES			1080
#define ENDSTOP_RATIO		0.01
#define SECURITY_TORQUE		30			// 上电的时候钳位的力矩
#define HOME_DEADBAND		30			// 度，回零的时候角度与零位的差值在这个范围内就算回到的零位


extern const float AXIS_DAMPER_RATIO;
extern const biquad_constant_t filterSpeedCst[4];
extern const biquad_constant_t filterAccelCst[4];
extern const biquad_constant_t filterDamperCst;

typedef struct Control_t {
	bool emergency			;
	bool usb_disabled		;
	bool update_disabled	;
	bool request_update_disabled	;
//	bool usb_update_flag = false;
//	bool update_flag = false;
	bool resetEncoder		;
}Control_t;

typedef struct AxisFlashAddrs
{
	uint16_t addr_config		;	// = ADR_AXIS1_CONFIG;
	uint16_t addr_maxSpeed		;	// = ADR_AXIS1_MAX_SPEED;
	uint16_t addr_maxAccel		;	// = ADR_AXIS1_MAX_ACCEL;
	//目前的可设置参数
	uint16_t addr_degrees		;
	uint16_t addr_power			;	
	uint16_t addr_fxratio		;
	uint16_t addr_friction		;
	uint16_t addr_damper		;
	uint16_t addr_inertia		;
	uint16_t addr_idlespring	;
	uint16_t addr_invertx		;

	uint16_t addr_endstop		;	// = ADR_AXIS1_ENDSTOP;
	uint16_t addr_encoderRatio	;	// = ADR_AXIS1_ENC_RATIO;

	uint16_t addr_speedAccelFilter;	// = ADR_AXIS1_SPEEDACCEL_FILTER;
	uint16_t addr_homePosL16	;	// 原点位置的低16位，
	uint16_t addr_homePosH16	;	// 原点位置的高16位
} AxisFlashAddrs;

typedef struct AxisConfig
{
	uint8_t drvtype				;
	uint8_t enctype				;
	uint16_t maxSpeedDegS		;			// Set to non zero to enable. example 1000. 8b * 10?
	float	 maxAccelDegSS		;
	uint16_t degrees			;			// How many degrees of range for the full gamepad range
	uint8_t power				;
	uint8_t fxratio				;
	uint8_t friction			;
	uint8_t damper				;
	uint8_t inertia				;
	uint8_t idlespring			;
	uint8_t invertx				;
	uint8_t endstop				;
	uint16_t encoderRatio		;
	uint16_t speedAccelFilter	;
	int16_t homePosL16			;
	int16_t homePosH16			;
	
}AxisConfig;

typedef struct metric_t {
	uint32_t tick				;		// 记录时间，ms
	int32_t posRaw				;		// 伺服方向，定制伺服是在整个可转动范围内-32768~32767，迈信标准伺服是0x20000一圈
	int32_t posScale			;		// 游戏方向，缩放到-32768 ~ 32767范围的位置。
	float posDegrees			;		// 游戏方向，以零位为中心，正方向增大，负方向减小。
	float speed					; 		// in deg/s
	float accel					;		// in deg/s²
	int32_t torque				;		// total of effect + endstop torque
}metric_t;


typedef struct axis_metric_t {
	metric_t current;
	metric_t previous;
}axis_metric_t;

typedef struct {
	uint8_t denominator	;	// = 0;
	uint8_t numerator	;	// = 0;
	float gearRatio		;	// = 1.0;
}GearRatio_t;

typedef struct Axis
{
	uint8_t axisId;
	AxisFlashAddrs* flashAddrs;
	volatile Control_t control;

	AxisConfig conf;
	int32_t homePos;
	bool outOfBounds;

	float encoderOffset;			// Offset for absolute encoders
	uint16_t degreesOfRotation;		// How many degrees of range for the full gamepad range
	uint16_t nextDegreesOfRotation;	// Buffer when changing range

	// Limiters
	uint32_t maxTorqueRateMS;		// 8b * 128?

	float spdlimitreducerI;
	
	// Merge normalized
	axis_metric_t metric;
	int32_t effectTorque;
	int32_t axisEffectTorque;
	uint8_t fx_ratio_i;				// Reduce effects to a certain ratio of the total power to have a margin for the endstop. 80% = 204
	uint16_t power;
	float torqueScaler;				// power * fx_ratio as a ratio between 0 & 1
	float et_torqueScaler;
	bool invertAxis;
	uint8_t endstopStrength;		// Sets how much extra torque per count above endstop is added. High = stiff endstop. Low = softer
	uint16_t maxSpeed;

	int16_t idlespringclip;	
	float idlespringdeadband;
	float idlespringscale;

	// TODO tune these and check if it is really stable and beneficial to the FFB. index 4 placeholder

	uint8_t filterProfileId;		// Default medium (1) as this is the most common encoder resolution and users can go lower or higher if required.
	uint8_t damperIntensity;
	fast_avgf_t spdlimiterAvg;

	Biquad speedFilter;				// = Biquad(BiquadType::lowpass, filterSpeedCst[filterProfileId].freq/filter_f, filterSpeedCst[filterProfileId].q/100.0, 0.0);
	Biquad accelFilter;				// = Biquad(BiquadType::lowpass, filterAccelCst[filterProfileId].freq/filter_f, filterAccelCst[filterProfileId].q/100.0, 0.0);
	Biquad damperFilter;			// = Biquad(BiquadType::lowpass, filterDamperCst.freq/filter_f, filterDamperCst.q / 100.0, 0.0); // enable on class constructor

	GearRatio_t gearRatio;

}Axis;

void Axis_Init(Axis* axis);
void AxisCalculateAxisEffects(Axis* axis,bool ffb_on);
void AxisSetEffectTorque(Axis* axis,int32_t torque);
metric_t* AxisGetMetrics(Axis* axis);
void AxisPrepareForUpdate(Axis* axis);
void AxisUpdateDriveTorque(Axis* axis);
void AxisUpdateMetrics(Axis* axis,float new_pos);
void AxisSetHome(Axis* axis,int32_t pos);
void AxisSetDegrees(Axis* axis,uint16_t degrees);
void AxisSetEndStopStrength(Axis* axis,uint16_t strength);
void AxisSetMaxSpeed(Axis* axis,uint16_t maxSpeed);
void AxisSetPower(Axis* axis,uint16_t power);
void AxisSetFxRatio(Axis* axis,uint8_t val) ;
void AxisSetFriction(Axis* axis,uint8_t friction);
void AxisSetDamperStrength(Axis* axis,uint8_t damper);
void AxisSetIdleSpringStrength(Axis* axis,uint8_t spring);
void AxisSetInvertx(Axis* axis,bool invertx);

#endif /* SRC_AXIS_H_ */

